﻿#include "boxline.h"
#include "systemconfiguration.h"
#include "component_log.h"
#include "componentmanager.h"
#include "../algorithm/AlgorithmManager.h"
#include "wcs/WCSSend.h"

Boxline::Boxline(const QString& id) : m_id(id)
{}

Boxline::~Boxline()
{}

bool Boxline::Init()
{
	auto rob = syscfg->GetRobotSetting();
	m_robName = rob.id;

	auto setting = syscfg->GetBoxlineCfg(m_id);
	if (setting.id != m_id) {
		log_error("{} Get config failed", m_id);
		return false;
	}

	m_type = (BoxlineType)setting.type;
	if (m_type >= BoxlineType::boxline_type_max) {
		log_error("输送线{}类型错误", m_id);
		return false;
	}
	m_di = setting.di;
	m_dival = setting.dival;
	if (m_di.size() != m_dival.size()) {
		log_error("DI配置错误，数量异常");
		return false;
	}
	for (int i = 0; i < m_di.size(); i++) {
		if (m_di[i] <= 0 || m_di[i] > 32) {
			log_error("DI配置错误，DI范围异常(0-32)");
			return false;
		}
		if (m_dival[i] != '0' && m_dival[i] != '1') {
			log_error("DI配置错误，正常值异常，只能为0或1");
			return false;
		}
	}

	m_rob = component->GetComponent(Component::component_robot);
	if (m_rob == nullptr) {
		log_error("{} init failed, get robot componet failed", m_id);
		return false;
	}


	m_wcs = dynamic_cast<WCSSend*>(component->GetComponent(Component::component_wcsSend));
	if (m_wcs == nullptr) {
		log_error("Get wcs send comp failed");
		return false;
	}
	auto algManger = dynamic_cast<AlgorithmManager*>(component->GetComponent(Component::component_algorithm));
	if (algManger == nullptr) {
		log_error("{} init failed, get alg manager componet failed", m_id);
		return false;
	}

	m_alg = algManger->GetAlgObjectByName(setting.alg);
	if (m_alg == nullptr) {
		log_error("{} init failed, get alg [{}]failed", m_id, setting.alg);
		return false;
	}
	AlgInit algparam = {};
	algparam.base = setting.base;
	algparam.rang = {};
	QString other = QString::number(setting.dir);
	if (!m_alg->Init(algparam, other)) {
		log_error("{} init failed, alg [{}] init failed", m_id, setting.alg);
		return false;
	}

	m_state.state = false;	//初始状态设置有货，但货物数量为0
	m_state.num = 0;
	m_state.sku.clear();
	log_trace("{} init successed", m_id);
	return true;
}
// 判读是否有货, 有货返回1, 无货返回0, 异常返回 -1
int Boxline::GetState()
{
	switch (m_type) {
	case BoxlineType::boxline_put_sku:
	{
		if (!GetBoxlineState()) {
			log_error("{}获取状态失败", m_id);
			return -1;
		}
		int ret = CheckBoxlineDI(m_state.state);
		if (ret == 0) {	// DI校验正常
			return m_state.state ? 1 : 0;
		} else if (ret == 1) {	// DI 校验不通过，直接返回有货
			return 1;
		} else {
			log_error("{}获取状态DI校验异常", m_id);
			return -1;
		}

		break;
	}
	case BoxlineType::boxline_pick_sku:
		return m_state.state ? 1 : 0;
		break;
	default:
		break;
	}
	log_error("错误的输送类型");
	return true;
}

// 获取当前位置货物数量
int Boxline::GetSkuNum()
{
	return m_state.num;
}

/*更新物料尺寸信息*/
bool Boxline::UpdateSkuInfo(const SkuInfo& info)
{
	auto& sku = m_state.sku;
	if (sku.valied) {
		if (sku.skuNo != info.skuNo) {
			log_error("input skuNo {} is not equal to {}", info.skuNo, sku.skuNo);
			return false;
		}
	}
	m_state.sku = info;
	AlgSkuInfo alg = {};
	alg.size = sku.size;
	alg.stackType = sku.pos;
	QString err;
	bool ret = m_alg->InitSkuInfo(alg, err);
	if (!ret) {
		log_error("{} alg init sku info failed, err = {}", m_id, err);
	}
	return ret;
}
/*算法计算, index: 抓取次数， num: 抓取数量*/
bool Boxline::CalculateNextGrab(int index, int num, QString& err)
{
	bool ret = m_alg->Calculate(QString::number(num), m_algResult.pos, err);
	if (ret) {
		m_algResult.index = index;
		log_trace("{} calculate position successed", m_id);
	} else {
		m_algResult.pos = {};
		m_algResult.index = -1;
		log_error("{} calculate position failed,err = {}", m_id, err);
	}
	return ret;
}
/*获取抓取坐标, index: 抓取次数， pos:坐标*/
bool Boxline::GetSkuPosition(int index, Position& pos)
{
	if (index == m_algResult.index) {
		pos = m_algResult.pos;
		return true;
	}
	pos = {};
	log_error("{} calculate index is {}, not equal to input index {}", m_id, m_algResult.index, index);
	return false;
}

// 设置物料移走或到位
bool Boxline::SetSkuMoved()
{
	m_state.state = false;
	m_state.num = 0;
	m_state.sku.clear();
	return true;
}
bool Boxline::SetSkuArrived(int num, QString SkuNo)
{
	log_trace("{} sku arrived num = {}", m_id, num);
	if (num > 0) {
		m_state.state = true;
		m_state.num = num;
		if (!SkuNo.isEmpty()) {
			m_state.sku.skuNo = SkuNo;
			m_state.sku.valied = true;
		}
		return true;
	} else {
		return false;
	}
}

// 改变物料到位状态
bool Boxline::ChangeBoxlineState(int state)
{
	m_state.state = state == 0 ? true : false;
	return true;
}

structJ(GetStateInfo,
	mapping(QString, putNo);
mapping(QString, robNo);
prop(&robNo, &putNo);
);
structJ(StateInfo,
	mapping(int, status);
prop(&status);
);

structJ(AnsInfo,
	mapping(int, code);
mapping(QString, msg);
mapping(StateInfo, data);
prop(&code, &msg, &data);
);
#define GET_STATE ("/place/status")
bool Boxline::GetBoxlineState()
{
	GetStateInfo data = {};
	data.putNo = m_id;
	data.robNo = m_robName;
	QByteArray ans;
	bool ret = m_wcs->SendToWCS(GET_STATE, data.toByteArray(), ans);
	if (!ret) {
		log_error("获取托盘信息失败：{}", ans);
		return false;
	}

	AnsInfo info = {};
	info.fromJson(ans);
	if (info.code() != 0) {
		log_error("获取托盘信息失败：{}", info.msg());
		return false;
	}

	if (info.data().status() == 1) { // 放货输送， 可用状态表示输送线位置没有货物
		m_state.state = false;
		m_state.num = 0;
	} else if (info.data().status() == -1) { // 放货输送，不可用状态表示输送线位置有货
		m_state.state = true;
		m_state.num = 1;
	} else {
		log_error("WCS返回的输送线状态异常：{}", info.data().status());
		return false;
	}
	return true;
}

int Boxline::CheckBoxlineDI(const bool state)
{
	if (m_di.size() <= 0) {	// 未配置DI时直接返回校验通过
		return 0;
	}
	QByteArray output;
	m_rob->ExcuteCmd((int)RobotCmdType::robot_cmd_get_di, QByteArray::number(-1), output);
	if (output.size() != 32) {
		log_error("获取机械手的di数据异常， {}", output);
		return -1;
	}
	bool check = true;
	QString all = output;
	for (int i = 0; i < m_di.size(); i++) {
		if (all[m_di[i] - 1] != m_dival[i]) {
			check = false;
			break;
		}
	}

	if (check) {	// DI值等于正常值，说明输送无货
		if (state) {
			log_error("DI校验无货,但是实际有货， output = {}", all);
			return 1;
		} else {
			return 0;
		}
	} else {	// DI 不等于正常值，说明输送有货
		if (!state) {
			log_error("DI校验有货,但是实际无货， output = {}", all);
			return 1;
		} else {
			return 0;
		}
	}

}
